#include "RectangleBody.h"
#include <cmath>
#include <qmath.h>

RectangleBody::RectangleBody(Vector* size) {
	this->setId(rand());
	this->size = size;

}


void RectangleBody::calcAabb() {
	QQuaternion quaternion = this->getRotation();
	Point position = *this->getPosition();

	if (nullptr != this->upPosition) {
		position += *this->upPosition;
	}

	if (nullptr != this->upRotation) {
		quaternion = *this->upRotation * quaternion;

	}

	Aabb* aabb = this->getAabb();
	aabb->setPosition(position);
	float halfX = size->x() / 2;
	float halfY = size->y() / 2;
	float halfZ = size->z() / 2;
	QVector3D points[8];
	points[0] = quaternion.rotatedVector(QVector3D(halfX, halfY, halfZ));
	points[1] = quaternion.rotatedVector(QVector3D(-halfX, halfY, halfZ));
	points[2] = quaternion.rotatedVector(QVector3D(halfX, -halfY, halfZ));
	points[3] = quaternion.rotatedVector(QVector3D(-halfX, -halfY, halfZ));

	points[4] = quaternion.rotatedVector(QVector3D(halfX, halfY, -halfZ));
	points[5] = quaternion.rotatedVector(QVector3D(-halfX, halfY, -halfZ));
	points[6] = quaternion.rotatedVector(QVector3D(halfX, -halfY, -halfZ));
	points[7] = quaternion.rotatedVector(QVector3D(-halfX, -halfY, -halfZ));

	float minX = std::min({ points[0].x(),points[1].x(), points[2].x(), points[3].x(), points[4].x(), points[5].x(), points[6].x(), points[7].x() });
	float minY = std::min({ points[0].y(),points[1].y(), points[2].y(), points[3].y(), points[4].y(), points[5].y(), points[6].y(), points[7].y() });
	float minZ = std::min({ points[0].z(),points[1].z(), points[2].z(), points[3].z(), points[4].z(), points[5].z(), points[6].z(), points[7].z() });
	float maxX = std::max({ points[0].x(),points[1].x(), points[2].x(), points[3].x(), points[4].x(), points[5].x(), points[6].x(), points[7].x() });
	float maxY = std::max({ points[0].y(),points[1].y(), points[2].y(), points[3].y(), points[4].y(), points[5].y(), points[6].y(), points[7].y() });
	float maxZ = std::max({ points[0].z(),points[1].z(), points[2].z(), points[3].z(), points[4].z(), points[5].z(), points[6].z(), points[7].z() });
	
	Point* aabbPoints = new Point[8];
	aabbPoints[0] = Point(points[0].x() + position.x(), points[0].y() + position.y(), points[0].z() + position.z());
	aabbPoints[1] = Point(points[1].x() + position.x(), points[1].y() + position.y(), points[1].z() + position.z());
	aabbPoints[2] = Point(points[2].x() + position.x(), points[2].y() + position.y(), points[2].z() + position.z());
	aabbPoints[3] = Point(points[3].x() + position.x(), points[3].y() + position.y(), points[3].z() + position.z());
	aabbPoints[4] = Point(points[4].x() + position.x(), points[4].y() + position.y(), points[4].z() + position.z());
	aabbPoints[5] = Point(points[5].x() + position.x(), points[5].y() + position.y(), points[5].z() + position.z());
	aabbPoints[6] = Point(points[6].x() + position.x(), points[6].y() + position.y(), points[6].z() + position.z());
	aabbPoints[7] = Point(points[7].x() + position.x(), points[7].y() + position.y(), points[7].z() + position.z());

	int indexSize = 36;
	int* indies = new int[indexSize];
	int index = 0;
	indies[index++] = 0;
	indies[index++] = 1;
	indies[index++] = 2;
	
	indies[index++] = 2;
	indies[index++] = 1;
	indies[index++] = 3;
	
	indies[index++] = 4;
	indies[index++] = 6;
	indies[index++] = 5;
	
	indies[index++] = 5;
	indies[index++] = 6;
	indies[index++] = 7;
	
	indies[index++] = 4;
	indies[index++] = 0;
	indies[index++] = 6;
	
	indies[index++] = 6;
	indies[index++] = 0;
	indies[index++] = 2;
	
	indies[index++] = 1;
	indies[index++] = 5;
	indies[index++] = 3;

	indies[index++] = 3;
	indies[index++] = 5;
	indies[index++] = 7;

	indies[index++] = 1;
	indies[index++] = 5;
	indies[index++] = 0;

	indies[index++] = 0;
	indies[index++] = 5;
	indies[index++] = 4;


	indies[index++] = 7;
	indies[index++] = 6;
	indies[index++] = 3;

	indies[index++] = 3;
	indies[index++] = 6;
	indies[index++] = 2;

	this->aabb->setMin(Point(minX + position.x(), minY + position.y(), minZ + position.z()));
	this->aabb->setMax(Point(maxX + position.x(), maxY + position.y(), maxZ + position.z()));
	this->aabb->setIndies(indies);
	this->aabb->setPointSize(8);
	this->aabb->setIndexSize(indexSize);
	this->aabb->setPoints(aabbPoints);

}
